// The sample robotarm model.  You should build a file
// very similar to this for when you make your model.

#pragma warning(disable : 4018)
#pragma warning(disable : 4267)
#pragma warning(disable : 4311)
#pragma warning(disable : 4312)
#pragma warning(disable : 4244)
#pragma warning(disable : 4305)

#include "modelerview.h"
#include "modelerapp.h"
#include "modelerdraw.h"
#include "particleSystem.h"
#include "HandleManager.h"

#include <FL/gl.h>
#include <cstdlib>

//using namespace std;
#define M_PI 3.1415926
#define M_DEFAULT 2.0f
#define M_OFFSET 3.0f
#define P_OFFSET 0.3f
#define MAX_VEL 200
#define MIN_STEP 0.1

//DOFs
#define pelvis_trans_x 0
#define pelvis_trans_y 1
#define pelvis_trans_z 2
#define pelvis_euler_x 3
#define pelvis_euler_y 4
#define pelvis_euler_z 5
#define l_thigh_euler_x 6
#define l_thigh_euler_y 7
#define l_thigh_euler_z 8
#define l_knee_euler_z 9
#define l_ankle_euler_z 10
#define l_ankle_euler_y 11
#define r_thigh_euler_x 12
#define r_thigh_euler_y 13
#define r_thigh_euler_z 14
#define r_knee_euler_z 15
#define r_ankle_euler_z 16
#define r_ankle_euler_y 17
#define abdomen_euler_x 18
#define abdomen_euler_z 19
#define spine_euler_y 20
#define neck_euler_z 21
#define neck_euler_x 22
#define l_scapula_euler_x 23
#define l_bicep_euler_x 24
#define l_bicep_euler_y 25
#define l_bicep_euler_z 26
#define l_elbow_euler_z 27
#define l_wrist_euler_x 28
#define r_scapula_euler_x 29
#define r_bicep_euler_x 30
#define r_bicep_euler_y 31
#define r_bicep_euler_z 32
#define r_elbow_euler_z 33
#define r_wrist_euler_x 34


// This is a list of the controls for the RobotArm
// We'll use these constants to access the values 
// of the controls from the user interface.
enum RobotArmControls
{ 
	BASE_ROTATION=0, LOWER_TILT, UPPER_TILT, CLAW_ROTATION,
	BASE_LENGTH, LOWER_LENGTH, UPPER_LENGTH, 

	NUMCONTROLS, 
};

Mat4d translate(float x, float y, float z)
{
	Mat4d m;
	m = vl_1;
	m[0][3] = x;
	m[1][3] = y;
	m[2][3] = z;
	return m;
}

Mat4d rotateX(float a)
{
	Mat4d m;
	m = vl_1;
	m[1][1] = cos(a);
	m[1][2] = -sin(a);
	m[2][1] = sin(a);
	m[2][2] = cos(a);
	return m;
}

Mat4d drotateX(float a)
{
	Mat4d m;
	m = vl_0;
	m[1][1] = -sin(a);
	m[1][2] = -cos(a);
	m[2][1] = cos(a);
	m[2][2] = -sin(a);
	return m;
}

Mat4d rotateY(float a)
{
	Mat4d m;
	m = vl_1;
	m[0][0] = cos(a);
	m[0][2] = sin(a);
	m[2][0] = -sin(a);
	m[2][2] = cos(a);
	return m;
}

Mat4d drotateY(float a)
{
	Mat4d m;
	m = vl_0;
	m[0][0] = -sin(a);
	m[0][2] = cos(a);
	m[2][0] = -cos(a);
	m[2][2] = -sin(a);
	return m;
}


Mat4d rotateZ(float a)
{
	Mat4d m;
	m = vl_1;
	m[0][0] = cos(a);
	m[0][1] = -sin(a);
	m[1][0] = sin(a);
	m[1][1] = cos(a);
	return m;
}
Mat4d drotateZ(float a)
{
	Mat4d m;
	m = vl_0;
	m[0][0] = -sin(a);
	m[0][1] = -cos(a);
	m[1][0] = cos(a);
	m[1][1] = -sin(a);
	return m;
}

float alpha_param;
float error_param;


void ground(float h);
void ellipsoid(float a, float b, float c);
void base(float h);
void rotation_base(float h);
void lower_arm(float h);
void upper_arm(float h);
void claw(float h);
void y_box(float h);

// To make a RobotArm, we inherit off of ModelerView
class RobotArm : public ModelerView 
{
	float pelvis_width;
	float thigh_len;
	float shin_len;
	float foot_len;
	float pelvis_height;
	float abdomen_len;
	float spine_len;
	float head_len;
	float scapula_off;
	float scapula_len;
	float bicep_len;
	float forearm_len;
	float hand_len;

	int handles;
	int dofs;
	
	Vec4d J [41][35];
	float qDOF[35];

	Mat4d m_dx;
	Mat4d m_dy;
	Mat4d m_dz;


	void initPoseVars();
	void computePose();
	void computeJacobian();
	void optimize();
	void checkError();

	HandleManager *hm;
	std::vector<Handle> ptrHandles;

public:
	RobotArm(int x, int y, int w, int h, char *label) 
		: ModelerView(x,y,w,h,label) {

			ptrHandles = ModelerApplication::Instance()->GetHandleManager()->getHandles();

			qDOF[pelvis_trans_x] = 0.0000;
			qDOF[pelvis_trans_y] = 0.8500;
			qDOF[pelvis_trans_z] = 0.0000;
			qDOF[pelvis_euler_x] = 0.0000;
			qDOF[pelvis_euler_y] = 1.5;
			qDOF[pelvis_euler_z] = 0.0000;
			pelvis_width = 0.1088;
			qDOF[l_thigh_euler_x] = -0.0512;
			qDOF[l_thigh_euler_y] = 0.0897;
			qDOF[l_thigh_euler_z] = 0.1403;
			thigh_len = 0.3521;
			qDOF[l_knee_euler_z] = -0.17103790795454556;
			shin_len = 0.4237;
			qDOF[l_ankle_euler_z] = 0.0000;
			qDOF[l_ankle_euler_y] = 0.0000;
			foot_len = 0.1408;
			qDOF[r_thigh_euler_x] = 0.0512;
			qDOF[r_thigh_euler_y] = -0.0897;
			qDOF[r_thigh_euler_z] = 0.1403;
			qDOF[r_knee_euler_z] = -0.17103790795454556;
			qDOF[r_ankle_euler_z] = 0.0000;
			qDOF[r_ankle_euler_y] = 0.0000;
			pelvis_height = 0.0562;
			qDOF[abdomen_euler_x] = 0.0000;
			qDOF[abdomen_euler_z] = 0.0000;
			abdomen_len = 0.2507;
			qDOF[spine_euler_y] = 0.0;
			spine_len = 0.2016;
			qDOF[neck_euler_z] = 0.0000;
			qDOF[neck_euler_x] = 0.0000;
			head_len = 0.1765;
			scapula_off = 0.0253;
			qDOF[l_scapula_euler_x] = -0.13708837234090906;
			scapula_len = 0.1923;
			qDOF[l_bicep_euler_x] = 1.1054;
			qDOF[l_bicep_euler_y] = 0.1254;
			qDOF[l_bicep_euler_z] = -0.1372;
			bicep_len = 0.2616;
			qDOF[l_elbow_euler_z] = 0.7040176213636364;
			forearm_len = 0.2024;
			qDOF[l_wrist_euler_x] = 0.0000;
			hand_len = 0.1319;
			qDOF[r_scapula_euler_x] = 0.13708837234090906;
			qDOF[r_bicep_euler_x] = -1.1054;
			qDOF[r_bicep_euler_y] = -0.1254;
			qDOF[r_bicep_euler_z] = -0.1372;
			qDOF[r_elbow_euler_z] = 0.7040176213636364;
			qDOF[r_wrist_euler_x] = 0.0000;

			handles = 41;
			dofs = 35;

			initPoseVars();

	}
	virtual void draw();
};

// We need to make a creator function, mostly because of
// nasty API stuff that we'd rather stay away from.
ModelerView* createRobotArm(int x, int y, int w, int h, char *label)
{ 
	return new RobotArm(x,y,w,h,label); 
}

// We'll be getting the instance of the application a lot; 
// might as well have it as a macro.
#define VAL(x) (ModelerApplication::Instance()->GetControlValue(x))


// We are going to override (is that the right word?) the draw()
// method of ModelerView to draw out RobotArm
void RobotArm::draw()
{
	// This call takes care of a lot of the nasty projection 
	// matrix stuff
	ModelerView::draw();

	hm = ModelerApplication::Instance()->GetHandleManager();
	if(hm != NULL && t < 6.49) {
		hm->drawMarkers(t, ModelerApplication::Instance()->GetFps());
	}

	if (t < 6.49)
		computePose();

	static GLfloat lmodel_ambient[] = {0.4,0.4,0.4,1.0};

	// ground
	ground(-0.2);

	// root
	glTranslatef(qDOF[pelvis_trans_x], qDOF[pelvis_trans_y], qDOF[pelvis_trans_z]);
	glRotatef(qDOF[pelvis_euler_x]*180/M_PI, 1.0, 0.0, 0.0);	
	glRotatef(qDOF[pelvis_euler_y]*180/M_PI, 0.0, 1.0, 0.0);
	glRotatef(qDOF[pelvis_euler_z]*180/M_PI, 0.0, 0.0, 1.0);
	ellipsoid(1.0*pelvis_width, 1.0*pelvis_width, 2.2*pelvis_width);
	ptrHandles[LFWT].draw();
	ptrHandles[RFWT].draw();
	ptrHandles[LBWT].draw();
	ptrHandles[RBWT].draw();
	glPushMatrix();
		// h_thigh_left
		glTranslatef(0.1516*pelvis_width, -0.5307*pelvis_width, -0.8339*pelvis_width);
		glRotatef(qDOF[l_thigh_euler_x]*180/M_PI, 1,0,0);
		glRotatef(qDOF[l_thigh_euler_y]*180/M_PI, 0,1,0);
		glRotatef(qDOF[l_thigh_euler_z]*180/M_PI, 0,0,1);
		ptrHandles[LTHI].draw();
		ptrHandles[LKNE].draw();
		glPushMatrix();
			glTranslatef(0.0, -0.5*thigh_len, 0.0);
			ellipsoid(0.3*thigh_len, 1.0*thigh_len, 0.3*thigh_len);
		glPopMatrix();
		// h_shin_left
		glTranslatef(0.0000,-1.0000*thigh_len,0.0000);
		glRotatef(qDOF[l_knee_euler_z]*180/M_PI, 0.0, 0.0, 1.0);
		ptrHandles[LSHN].draw();
		ptrHandles[LANK].draw();
		glPushMatrix();
			glTranslatef(0.0, -0.5*shin_len, 0.0);
			ellipsoid(0.2*shin_len, 1.0*shin_len, 0.2*shin_len);
		glPopMatrix();
		// h_foot_left
		glTranslatef(0.0000,-1.0000*shin_len,0.0000);
		glRotatef(qDOF[l_ankle_euler_z]*180/M_PI, 0.0, 0.0, 1.0);
		glRotatef(qDOF[l_ankle_euler_y]*180/M_PI, 0.0, 1.0, 0.0);
		ptrHandles[LHEE].draw();
		ptrHandles[LTOE].draw();
		ptrHandles[LMT5].draw();
		glPushMatrix();
			glTranslatef(0.5*foot_len, 0.0, 0.0);
			ellipsoid(1.0*foot_len, 0.25*foot_len, 0.4*foot_len);
		glPopMatrix();
	glPopMatrix();
	glPushMatrix();
		// h_thigh_right
		glTranslatef(0.1516*pelvis_width, -0.5307*pelvis_width, 0.8339*pelvis_width);
		glRotatef(qDOF[r_thigh_euler_x]*180/M_PI, 1,0,0);
		glRotatef(qDOF[r_thigh_euler_y]*180/M_PI, 0,1,0);
		glRotatef(qDOF[r_thigh_euler_z]*180/M_PI, 0,0,1);
		ptrHandles[RTHI].draw();
		ptrHandles[RKNE].draw();
		glPushMatrix();
			glTranslatef(0.0, -0.5*thigh_len, 0.0);
			ellipsoid(0.3*thigh_len, 1.0*thigh_len, 0.3*thigh_len);
		glPopMatrix();
		// h_shin_right
		glTranslatef(0.0000,-1.0000*thigh_len,0.0000);
		glRotatef(qDOF[r_knee_euler_z]*180/M_PI, 0.0, 0.0, 1.0);
		ptrHandles[RSHN].draw();
		ptrHandles[RANK].draw();
		glPushMatrix();
			glTranslatef(0.0, -0.5*shin_len, 0.0);
			ellipsoid(0.2*shin_len, 1.0*shin_len, 0.2*shin_len);
		glPopMatrix();
		// h_foot_right
		glTranslatef(0.0000,-1.0000*shin_len,0.0000);
		glRotatef(qDOF[r_ankle_euler_z]*180/M_PI, 0.0, 0.0, 1.0);
		glRotatef(qDOF[r_ankle_euler_y]*180/M_PI, 0.0, 1.0, 0.0);
		ptrHandles[RHEE].draw();
		ptrHandles[RTOE].draw();
		ptrHandles[RMT5].draw();
		glPushMatrix();
			glTranslatef(0.5*foot_len, 0.0, 0.0);
			ellipsoid(1.0*foot_len, 0.25*foot_len, 0.4*foot_len);
		glPopMatrix();
	glPopMatrix();
	glPushMatrix();
		// abdomen
		glTranslatef(0.0000,1.0000*pelvis_height ,0.0000);
		glRotatef(qDOF[abdomen_euler_x]*180/M_PI, 1.0,0.0,0.0);
		glRotatef(qDOF[abdomen_euler_z]*180/M_PI, 0.0,0.0,1.0);
		ptrHandles[T10].draw();
		ptrHandles[STRN].draw();
		glPushMatrix();
			glTranslatef(0.0, 0.5*abdomen_len, 0.0);
			ellipsoid(0.5*abdomen_len, 1.0*abdomen_len, 0.7*abdomen_len);
		glPopMatrix();
		// h_spine
		glTranslatef(0.0000,1.0000*abdomen_len,0.0000);
		glRotatef(qDOF[spine_euler_y]*180/M_PI, 0.0,1.0,0.0);
		ptrHandles[C7].draw();
		ptrHandles[CLAV].draw();
		glPushMatrix();
			glTranslatef(0.0, 0.5*spine_len, 0.0);
			ellipsoid(0.5*spine_len, 1.0*spine_len, 0.5*spine_len);
		glPopMatrix();
		glPushMatrix();
			// h_head
			glTranslatef(0.0000,1.0000*spine_len,0.0000);
			glRotatef(qDOF[neck_euler_z]*180/M_PI, 0.0,0.0,1.0);
			glRotatef(qDOF[neck_euler_x]*180/M_PI, 1.0,0.0,0.0);
			ptrHandles[LFHD].draw();
			ptrHandles[RFHD].draw();
			ptrHandles[LBHD].draw();
			ptrHandles[RBHD].draw();
			glPushMatrix();
				glTranslatef(0.0, 0.5*head_len, 0.0);
				ellipsoid(0.65*head_len, 1.0*head_len, 0.7*head_len);
			glPopMatrix();
		glPopMatrix();
		glPushMatrix();
			// h_scapula_left
			glRotatef(-0.9423*180/M_PI, 1.0,0.0,0.0);
			glRotatef(qDOF[l_scapula_euler_x]*180/M_PI, 1.0,0.0,0.0);
			ptrHandles[LSHO].draw();
			glPushMatrix();
				glTranslatef(0.0, 0.5*scapula_len, 0.0);
				ellipsoid(0.5*scapula_len, 1.0*scapula_len, 0.5*scapula_len);
			glPopMatrix();
			// h_bicep_left
			glTranslatef(0.0, 1.0*scapula_len, 0.0);
			glRotatef(qDOF[l_bicep_euler_x]*180/M_PI, 1.0, 0.0, 0.0);
			glRotatef(qDOF[l_bicep_euler_y]*180/M_PI, 0.0, 1.0, 0.0);
			glRotatef(qDOF[l_bicep_euler_z]*180/M_PI, 0.0, 0.0, 1.0);
			ptrHandles[LUPA].draw();
			ptrHandles[LELB].draw();
			glPushMatrix();
				glTranslatef(0.0, -0.5*bicep_len, 0.0);
				ellipsoid(0.3*bicep_len, 1.0*bicep_len, 0.3*bicep_len);
			glPopMatrix();
			// h_forearm_left
			glTranslatef(0.0, -1.0*bicep_len, 0.0);
			glRotatef(qDOF[l_elbow_euler_z]*180/M_PI, 0.0,0.0,1.0);
			ptrHandles[LFRM].draw();
			ptrHandles[LWRA].draw();
			ptrHandles[LWRB].draw();
			glPushMatrix();
				glTranslatef(0.0, -0.5*forearm_len, 0.0);
				ellipsoid(0.25*forearm_len, 1.0*forearm_len, 0.25*forearm_len);
			glPopMatrix();
			// h_hand_left
			glTranslatef(0.0, -1.0*forearm_len, 0.0);
			glRotatef(qDOF[l_wrist_euler_x]*180/M_PI, 1.0,0.0,0.0);
			ptrHandles[LFIN].draw();
			glPushMatrix();
				glTranslatef(0.0, -0.5*hand_len, 0.0);
				ellipsoid(0.25*hand_len, 1.0*hand_len, 0.25*hand_len);
			glPopMatrix();
		glPopMatrix();
		glPushMatrix();
			// h_scapula_right
			glRotatef(0.9423*180/M_PI, 1.0,0.0,0.0);
			glRotatef(qDOF[r_scapula_euler_x]*180/M_PI, 1.0,0.0,0.0);
			ptrHandles[RBAC].draw();
			ptrHandles[RSHO].draw();
			glPushMatrix();
				glTranslatef(0.0, 0.5*scapula_len, 0.0);
				ellipsoid(0.5*scapula_len, 1.0*scapula_len, 0.5*scapula_len);
			glPopMatrix();
			// h_bicep_right
			glTranslatef(0.0, 1.0*scapula_len, 0.0);
			glRotatef(qDOF[r_bicep_euler_x]*180/M_PI, 1.0, 0.0, 0.0);
			glRotatef(qDOF[r_bicep_euler_y]*180/M_PI, 0.0, 1.0, 0.0);
			glRotatef(qDOF[r_bicep_euler_z]*180/M_PI, 0.0, 0.0, 1.0);
			ptrHandles[RUPA].draw();
			ptrHandles[RELB].draw();
			glPushMatrix();
				glTranslatef(0.0, -0.5*bicep_len, 0.0);
				ellipsoid(0.3*bicep_len, 1.0*bicep_len, 0.3*bicep_len);
			glPopMatrix();
			// h_forearm_right
			glTranslatef(0.0, -1.0*bicep_len, 0.0);
			glRotatef(qDOF[r_elbow_euler_z]*180/M_PI, 0.0,0.0,1.0);
			ptrHandles[RFRM].draw();
			ptrHandles[RWRA].draw();
			ptrHandles[RWRB].draw();
			glPushMatrix();
				glTranslatef(0.0, -0.5*forearm_len, 0.0);
				ellipsoid(0.25*forearm_len, 1.0*forearm_len, 0.25*forearm_len);
			glPopMatrix();
			// h_hand_right
			glTranslatef(0.0, -1.0*forearm_len, 0.0);
			glRotatef(qDOF[r_wrist_euler_x]*180/M_PI, 1.0,0.0,0.0);
			ptrHandles[RFIN].draw();
			glPushMatrix();
				glTranslatef(0.0, -0.5*hand_len, 0.0);
				ellipsoid(0.25*hand_len, 1.0*hand_len, 0.25*hand_len);
			glPopMatrix();
		glPopMatrix();

	glPopMatrix();

	//*** DON'T FORGET TO PUT THIS IN YOUR OWN CODE **/
	endDraw();
}

void RobotArm::initPoseVars()
{
	//Partial Derivatives:
	m_dx = vl_0;
	m_dx[0][3] = 1.0;
	m_dy = vl_0;
	m_dy[1][3] = 1.0;
	m_dz = vl_0;
	m_dz[2][3] = 1.0;

	//Init Jacobian
	for(int c = 0; c < handles; c++) {
		for(int q = 0; q < dofs; q++) {
			J[c][q] = vl_0;
		}
	}
}

void RobotArm::computePose()
{
	computeJacobian();
	checkError();
}

/************************************************************************/
/* JACOBIAN                                                             */
/************************************************************************/
void RobotArm::computeJacobian()
{
	/************************************************************************/
	/* ROOT                                                                 */
	/************************************************************************/
	
	//Root Link Transformations
	Mat4d root_trans = translate(qDOF[pelvis_trans_x], qDOF[pelvis_trans_y], qDOF[pelvis_trans_z]);
	Mat4d root_rx = rotateX(qDOF[pelvis_euler_x]);
	Mat4d root_ry = rotateY(qDOF[pelvis_euler_y]);
	Mat4d root_rz = rotateZ(qDOF[pelvis_euler_z]);

	Mat4d root_matrix = root_trans * root_rx * root_ry * root_rz;
	Mat4d root_mTdx = m_dx * root_rx * root_ry * root_rz;
	Mat4d root_mTdy = m_dy * root_rx * root_ry * root_rz;
	Mat4d root_mTdz = m_dz * root_rx * root_ry * root_rz;
	Mat4d root_mRdx = root_trans * drotateX(qDOF[pelvis_euler_x]) * root_ry * root_rz;
	Mat4d root_mRdy = root_trans * root_rx * drotateY(qDOF[pelvis_euler_y]) * root_rz;
	Mat4d root_mRdz = root_trans * root_rx * root_ry * drotateZ(qDOF[pelvis_euler_z]);

	for(int c = LFWT; c <= RBWT; c++) {
		//Fill Jacobian d h(q)/ [dx, dy ,dz, ...]
		J[c][pelvis_trans_x] = root_mTdx * ptrHandles[c].position;
		J[c][pelvis_trans_y] = root_mTdy * ptrHandles[c].position;
		J[c][pelvis_trans_z] = root_mTdz * ptrHandles[c].position;
		J[c][pelvis_euler_x] = root_mRdx * ptrHandles[c].position;
		J[c][pelvis_euler_y] = root_mRdy * ptrHandles[c].position;
		J[c][pelvis_euler_z] = root_mRdz * ptrHandles[c].position;

		//h(q)
		Vec4d hq = root_matrix * ptrHandles[c].position;
		//c = h(q) - p
		Vec4d p = hm->getMarker(t, ModelerApplication::Instance()->GetFps(), c);
		if(p[0] == 0 && p[1] == 0 && p[2] == 0) {
			//cout << "***** OCCLUDED MARKER at: " << c << endl;
			ptrHandles[c].setC(Vec4d(vl_0));
		} else {
			ptrHandles[c].setC(hq - p);	
		}
	}


	/************************************************************************/
	/* h_thigh_left                                                         */
	/************************************************************************/

	//h_thigh_left Link Transformations
	Mat4d tl_trans = translate(0.1516*pelvis_width, -0.5307*pelvis_width, -0.8339*pelvis_width);
	Mat4d tl_rx = rotateX(qDOF[l_thigh_euler_x]);
	Mat4d tl_ry = rotateY(qDOF[l_thigh_euler_y]);
	Mat4d tl_rz = rotateZ(qDOF[l_thigh_euler_z]);

	Mat4d tl_matrix = tl_trans * tl_rx * tl_ry * tl_rz;
	Mat4d tl_mRdx = tl_trans * drotateX(qDOF[l_thigh_euler_x]) * tl_ry * tl_rz;
	Mat4d tl_mRdy = tl_trans * tl_rx * drotateY(qDOF[l_thigh_euler_y]) * tl_rz;
	Mat4d tl_mRdz = tl_trans * tl_rx * tl_ry * drotateZ(qDOF[l_thigh_euler_z]);

	for(int c = LTHI; c <= LKNE; c++) {
		//Fill Jacobian d h(q)/ [dx, dy ,dz, ...]
		J[c][pelvis_trans_x] = root_mTdx * tl_matrix * ptrHandles[c].position;
		J[c][pelvis_trans_y] = root_mTdy * tl_matrix * ptrHandles[c].position;
		J[c][pelvis_trans_z] = root_mTdz * tl_matrix * ptrHandles[c].position;
		J[c][pelvis_euler_x] = root_mRdx * tl_matrix * ptrHandles[c].position;
		J[c][pelvis_euler_y] = root_mRdy * tl_matrix * ptrHandles[c].position;
		J[c][pelvis_euler_z] = root_mRdz * tl_matrix * ptrHandles[c].position;
		J[c][l_thigh_euler_x] = root_matrix * tl_mRdx * ptrHandles[c].position;
		J[c][l_thigh_euler_y] = root_matrix * tl_mRdy * ptrHandles[c].position;
		J[c][l_thigh_euler_z] = root_matrix * tl_mRdz * ptrHandles[c].position;

		//h(q)
		Vec4d hq = root_matrix * tl_matrix * ptrHandles[c].position;
		//c = h(q) - p
		Vec4d p = hm->getMarker(t, ModelerApplication::Instance()->GetFps(), c);
		if(p[0] == 0 && p[1] == 0 && p[2] == 0) {
			//cout << "***** OCCLUDED MARKER at: " << c << endl;
			ptrHandles[c].setC(Vec4d(vl_0));
		} else {
			ptrHandles[c].setC(hq - p);	
		}
	}


	/************************************************************************/
	/* h_thigh_right                                                         */
	/************************************************************************/

	//h_thigh_right Link Transformations
	Mat4d tr_trans = translate(0.1516*pelvis_width, -0.5307*pelvis_width, 0.8339*pelvis_width);
	Mat4d tr_rx = rotateX(qDOF[r_thigh_euler_x]);
	Mat4d tr_ry = rotateY(qDOF[r_thigh_euler_y]);
	Mat4d tr_rz = rotateZ(qDOF[r_thigh_euler_z]);

	Mat4d tr_matrix = tr_trans * tr_rx * tr_ry * tr_rz;
	Mat4d tr_mRdx = tr_trans * drotateX(qDOF[r_thigh_euler_x]) * tr_ry * tr_rz;
	Mat4d tr_mRdy = tr_trans * tr_rx * drotateY(qDOF[r_thigh_euler_y]) * tr_rz;
	Mat4d tr_mRdz = tr_trans * tr_rx * tr_ry * drotateZ(qDOF[r_thigh_euler_z]);

	for(int c = RTHI; c <= RKNE; c++) {
		//Fill Jacobian d h(q)/ [dx, dy ,dz, ...]
		J[c][pelvis_trans_x] = root_mTdx * tr_matrix * ptrHandles[c].position;
		J[c][pelvis_trans_y] = root_mTdy * tr_matrix * ptrHandles[c].position;
		J[c][pelvis_trans_z] = root_mTdz * tr_matrix * ptrHandles[c].position;
		J[c][pelvis_euler_x] = root_mRdx * tr_matrix * ptrHandles[c].position;
		J[c][pelvis_euler_y] = root_mRdy * tr_matrix * ptrHandles[c].position;
		J[c][pelvis_euler_z] = root_mRdz * tr_matrix * ptrHandles[c].position;
		J[c][r_thigh_euler_x] = root_matrix * tr_mRdx * ptrHandles[c].position;
		J[c][r_thigh_euler_y] = root_matrix * tr_mRdy * ptrHandles[c].position;
		J[c][r_thigh_euler_z] = root_matrix * tr_mRdz * ptrHandles[c].position;

		//h(q)
		Vec4d hq = root_matrix * tr_matrix * ptrHandles[c].position;
		//c = h(q) - p
		Vec4d p = hm->getMarker(t, ModelerApplication::Instance()->GetFps(), c);
		if(p[0] == 0 && p[1] == 0 && p[2] == 0) {
			//cout << "***** OCCLUDED MARKER at: " << c << endl;
			ptrHandles[c].setC(Vec4d(vl_0));
		} else {
			ptrHandles[c].setC(hq - p);	
		}
	}


	/************************************************************************/
	/* h_shin_left                                                          */
	/************************************************************************/

	//h_shin_left Link transformations
	Mat4d sl_trans = translate(0.0000,-1.0000*thigh_len,0.0000);
	Mat4d sl_rz = rotateZ(qDOF[l_knee_euler_z] );

	Mat4d sl_matrix = sl_trans * sl_rz;
	Mat4d sl_mRdz = sl_trans * drotateZ(qDOF[l_knee_euler_z]);

	//Concatenate Matrix
	Mat4d tl_sl_matrix = tl_matrix * sl_matrix;

	for(int c = LSHN; c <= LANK; c++) {
		//Fill Jacobian d h(q)/ [dx, dy ,dz, ...]
		J[c][pelvis_trans_x] = root_mTdx * tl_sl_matrix * ptrHandles[c].position;
		J[c][pelvis_trans_y] = root_mTdy * tl_sl_matrix * ptrHandles[c].position;
		J[c][pelvis_trans_z] = root_mTdz * tl_sl_matrix * ptrHandles[c].position;
		J[c][pelvis_euler_x] = root_mRdx * tl_sl_matrix * ptrHandles[c].position;
		J[c][pelvis_euler_y] = root_mRdy * tl_sl_matrix * ptrHandles[c].position;
		J[c][pelvis_euler_z] = root_mRdz * tl_sl_matrix * ptrHandles[c].position;
		J[c][l_thigh_euler_x] = root_matrix * tl_mRdx * sl_matrix * ptrHandles[c].position;
		J[c][l_thigh_euler_y] = root_matrix * tl_mRdy * sl_matrix * ptrHandles[c].position;
		J[c][l_thigh_euler_z] = root_matrix * tl_mRdz * sl_matrix * ptrHandles[c].position;
		J[c][l_knee_euler_z] = root_matrix * tl_matrix * sl_mRdz * ptrHandles[c].position;

		//h(q)
		Vec4d hq = root_matrix * tl_sl_matrix * ptrHandles[c].position;
		//c = h(q) - p
		Vec4d p = hm->getMarker(t, ModelerApplication::Instance()->GetFps(), c);
		if(p[0] == 0 && p[1] == 0 && p[2] == 0) {
			//cout << "***** OCCLUDED MARKER at: " << c << endl;
			ptrHandles[c].setC(Vec4d(vl_0));
		} else {
			ptrHandles[c].setC(hq - p);	
		}
	}


	/************************************************************************/
	/* h_shin_right                                                         */
	/************************************************************************/

	//h_shin_right Link transformations
	Mat4d sr_trans = translate(0.0000,-1.0000*thigh_len,0.0000);
	Mat4d sr_rz = rotateZ(qDOF[r_knee_euler_z] );

	Mat4d sr_matrix = sr_trans * sr_rz;
	Mat4d sr_mRdz = sr_trans * drotateZ(qDOF[r_knee_euler_z]);

	//Concatenate Matrix
	Mat4d tr_sr_matrix = tr_matrix * sr_matrix;


	for(int c = RSHN; c <= RANK; c++) {
		//Fill Jacobian d h(q)/ [dx, dy ,dz, ...]
		J[c][pelvis_trans_x] = root_mTdx * tr_sr_matrix * ptrHandles[c].position;
		J[c][pelvis_trans_y] = root_mTdy * tr_sr_matrix * ptrHandles[c].position;
		J[c][pelvis_trans_z] = root_mTdz * tr_sr_matrix * ptrHandles[c].position;
		J[c][pelvis_euler_x] = root_mRdx * tr_sr_matrix * ptrHandles[c].position;
		J[c][pelvis_euler_y] = root_mRdy * tr_sr_matrix * ptrHandles[c].position;
		J[c][pelvis_euler_z] = root_mRdz * tr_sr_matrix * ptrHandles[c].position;
		J[c][r_thigh_euler_x] = root_matrix * tr_mRdx * sr_matrix * ptrHandles[c].position;
		J[c][r_thigh_euler_y] = root_matrix * tr_mRdy * sr_matrix * ptrHandles[c].position;
		J[c][r_thigh_euler_z] = root_matrix * tr_mRdz * sr_matrix * ptrHandles[c].position;
		J[c][r_knee_euler_z] = root_matrix * tr_matrix * sr_mRdz * ptrHandles[c].position;

		//h(q)
		Vec4d hq = root_matrix * tr_sr_matrix * ptrHandles[c].position;
		//c = h(q) - p
		Vec4d p = hm->getMarker(t, ModelerApplication::Instance()->GetFps(), c);
		if(p[0] == 0 && p[1] == 0 && p[2] == 0) {
			//cout << "***** OCCLUDED MARKER at: " << c << endl;
			ptrHandles[c].setC(Vec4d(vl_0));
		} else {
			ptrHandles[c].setC(hq - p);	
		}
	}


	/************************************************************************/
	/* h_foot_left                                                          */
	/************************************************************************/

	//h_foot_left Link transformations
	Mat4d fl_trans = translate(0.0000,-1.0000*shin_len,0.0000);
	Mat4d fl_rz = rotateZ(qDOF[l_ankle_euler_z]);
	Mat4d fl_ry = rotateY(qDOF[l_ankle_euler_y]);

	Mat4d fl_matrix = fl_trans * fl_rz * fl_ry;
	Mat4d fl_mRdz = fl_trans * drotateZ(qDOF[l_ankle_euler_z]) * fl_ry;
	Mat4d fl_mRdy = fl_trans * fl_rz * drotateY(qDOF[l_ankle_euler_y]);

	//Concatenating Matrices:
	Mat4d tl_sl_fl_matrix = tl_sl_matrix * fl_matrix;

	for(int c = LHEE; c <= LMT5; c++) {
		//Fill Jacobian d h(q)/ [dx, dy ,dz, ...]
		J[c][pelvis_trans_x] = root_mTdx * tl_sl_fl_matrix * ptrHandles[c].position;
		J[c][pelvis_trans_y] = root_mTdy * tl_sl_fl_matrix * ptrHandles[c].position;
		J[c][pelvis_trans_z] = root_mTdz * tl_sl_fl_matrix * ptrHandles[c].position;
		J[c][pelvis_euler_x] = root_mRdx * tl_sl_fl_matrix * ptrHandles[c].position;
		J[c][pelvis_euler_y] = root_mRdy * tl_sl_fl_matrix * ptrHandles[c].position;
		J[c][pelvis_euler_z] = root_mRdz * tl_sl_fl_matrix * ptrHandles[c].position;
		J[c][l_thigh_euler_x] = root_matrix * tl_mRdx * sl_matrix * fl_matrix * ptrHandles[c].position;
		J[c][l_thigh_euler_y] = root_matrix * tl_mRdy * sl_matrix * fl_matrix * ptrHandles[c].position;
		J[c][l_thigh_euler_z] = root_matrix * tl_mRdz * sl_matrix * fl_matrix * ptrHandles[c].position;
		J[c][l_knee_euler_z] = root_matrix * tl_matrix * sl_mRdz * fl_matrix * ptrHandles[c].position;
		J[c][l_ankle_euler_z] = root_matrix * tl_sl_matrix * fl_mRdz * ptrHandles[c].position;
		J[c][l_ankle_euler_y] = root_matrix * tl_sl_matrix * fl_mRdy * ptrHandles[c].position;

		//h(q)
		Vec4d hq = root_matrix * tl_sl_matrix * fl_matrix * ptrHandles[c].position;
		//c = h(q) - p
		Vec4d p = hm->getMarker(t, ModelerApplication::Instance()->GetFps(), c);
		if(p[0] == 0 && p[1] == 0 && p[2] == 0) {
			//cout << "***** OCCLUDED MARKER at: " << c << endl;
			ptrHandles[c].setC(Vec4d(vl_0));
		} else {
			ptrHandles[c].setC(hq - p);	
		}
		
	}

	/************************************************************************/
	/* h_foot_right                                                          */
	/************************************************************************/

	//h_foot_right Link transformations
	Mat4d fr_trans = translate(0.0000,-1.0000*shin_len,0.0000);
	Mat4d fr_rz = rotateZ(qDOF[r_ankle_euler_z]);
	Mat4d fr_ry = rotateY(qDOF[r_ankle_euler_y]);

	Mat4d fr_matrix = fr_trans * fr_rz * fr_ry;
	Mat4d fr_mRdz = fr_trans * drotateZ(qDOF[r_ankle_euler_z]) * fr_ry;
	Mat4d fr_mRdy = fr_trans * fr_rz * drotateY(qDOF[r_ankle_euler_y]);

	//Concatenating Matrices:
	Mat4d tr_sr_fr_matrix = tr_sr_matrix * fr_matrix;

	for(int c = RHEE; c <= RMT5; c++) {
		//Fill Jacobian d h(q)/ [dx, dy ,dz, ...]
		J[c][pelvis_trans_x] = root_mTdx * tr_sr_fr_matrix * ptrHandles[c].position;
		J[c][pelvis_trans_y] = root_mTdy * tr_sr_fr_matrix * ptrHandles[c].position;
		J[c][pelvis_trans_z] = root_mTdz * tr_sr_fr_matrix * ptrHandles[c].position;
		J[c][pelvis_euler_x] = root_mRdx * tr_sr_fr_matrix * ptrHandles[c].position;
		J[c][pelvis_euler_y] = root_mRdy * tr_sr_fr_matrix * ptrHandles[c].position;
		J[c][pelvis_euler_z] = root_mRdz * tr_sr_fr_matrix * ptrHandles[c].position;
		J[c][r_thigh_euler_x] = root_matrix * tr_mRdx * sr_matrix * fr_matrix * ptrHandles[c].position;
		J[c][r_thigh_euler_y] = root_matrix * tr_mRdy * sr_matrix * fr_matrix * ptrHandles[c].position;
		J[c][r_thigh_euler_z] = root_matrix * tr_mRdz * sr_matrix * fr_matrix * ptrHandles[c].position;
		J[c][r_knee_euler_z] = root_matrix * tr_matrix * sr_mRdz * fr_matrix * ptrHandles[c].position;
		J[c][r_ankle_euler_z] = root_matrix * tr_sr_matrix * fr_mRdz * ptrHandles[c].position;
		J[c][r_ankle_euler_y] = root_matrix * tr_sr_matrix * fr_mRdy * ptrHandles[c].position;

		//h(q)
		Vec4d hq = root_matrix * tr_sr_matrix * fr_matrix * ptrHandles[c].position;
		//c = h(q) - p
		Vec4d p = hm->getMarker(t, ModelerApplication::Instance()->GetFps(), c);
		if(p[0] == 0 && p[1] == 0 && p[2] == 0) {
			//cout << "***** OCCLUDED MARKER at: " << c << endl;
			ptrHandles[c].setC(Vec4d(vl_0));
		} else {
			ptrHandles[c].setC(hq - p);	
		}	
	}


	/************************************************************************/
	/* Abdomen                                                              */
	/************************************************************************/

	//h_abdomen Link Transformations
	Mat4d abdomen_trans = translate(0.0000,1.0000*pelvis_height ,0.0000);
	Mat4d abdomen_rx = rotateX(qDOF[abdomen_euler_x]);
	Mat4d abdomen_rz = rotateZ(qDOF[abdomen_euler_z]);

	Mat4d abdomen_matrix = abdomen_trans * abdomen_rx * abdomen_rz;
	Mat4d abdomen_mRdx = abdomen_trans * drotateX(qDOF[abdomen_euler_x]) * abdomen_rz;
	Mat4d abdomen_mRdz = abdomen_trans * abdomen_rx * drotateZ(qDOF[abdomen_euler_z]);

	for(int c = T10; c <= STRN; c+=2) {
		//Fill Jacobian d h(q)/ [dx, dy ,dz, ...]
		J[c][pelvis_trans_x] = root_mTdx * abdomen_matrix * ptrHandles[c].position;
		J[c][pelvis_trans_y] = root_mTdy * abdomen_matrix * ptrHandles[c].position;
		J[c][pelvis_trans_z] = root_mTdz * abdomen_matrix * ptrHandles[c].position;
		J[c][pelvis_euler_x] = root_mRdx * abdomen_matrix * ptrHandles[c].position;
		J[c][pelvis_euler_y] = root_mRdy * abdomen_matrix * ptrHandles[c].position;
		J[c][pelvis_euler_z] = root_mRdz * abdomen_matrix * ptrHandles[c].position;
		J[c][abdomen_euler_x] = root_matrix * abdomen_mRdx * ptrHandles[c].position;
		J[c][abdomen_euler_z] = root_matrix * abdomen_mRdz * ptrHandles[c].position;

		//h(q)
		Vec4d hq = root_matrix * abdomen_matrix * ptrHandles[c].position;
		//c = h(q) - p
		Vec4d p = hm->getMarker(t, ModelerApplication::Instance()->GetFps(), c);
		if(p[0] == 0 && p[1] == 0 && p[2] == 0) {
			//cout << "***** OCCLUDED MARKER at: " << c << endl;
			ptrHandles[c].setC(Vec4d(vl_0));
		} else {
			ptrHandles[c].setC(hq - p);	
		}
			
	}


	/************************************************************************/
	/* Spine                                                                */
	/************************************************************************/
	
	//h_spine Link Transformations
	Mat4d spine_trans = translate(0.0000,1.0000*abdomen_len,0.0000);
	Mat4d spine_ry = rotateY(qDOF[spine_euler_y]);

	Mat4d spine_matrix = spine_trans * spine_ry;
	Mat4d spine_mRdy = spine_trans * drotateY(qDOF[spine_euler_y]);

	//Concatenation of matrices
	Mat4d abdomen_spine_matrix = abdomen_matrix * spine_matrix;
	Mat4d root_absRdx_spine_matrix = root_matrix * abdomen_mRdx * spine_matrix;
	Mat4d root_absRdz_spine_matrix = root_matrix * abdomen_mRdz * spine_matrix;

	for(int c = C7; c <= CLAV; c+=2) {
		//Fill Jacobian d h(q)/ [dx, dy ,dz, ...]
		J[c][pelvis_trans_x] = root_mTdx * abdomen_spine_matrix * ptrHandles[c].position;
		J[c][pelvis_trans_y] = root_mTdy * abdomen_spine_matrix * ptrHandles[c].position;
		J[c][pelvis_trans_z] = root_mTdz * abdomen_spine_matrix * ptrHandles[c].position;
		J[c][pelvis_euler_x] = root_mRdx * abdomen_spine_matrix * ptrHandles[c].position;
		J[c][pelvis_euler_y] = root_mRdy * abdomen_spine_matrix * ptrHandles[c].position;
		J[c][pelvis_euler_z] = root_mRdz * abdomen_spine_matrix * ptrHandles[c].position;
		J[c][abdomen_euler_x] = root_absRdx_spine_matrix * ptrHandles[c].position;
		J[c][abdomen_euler_z] = root_absRdz_spine_matrix * ptrHandles[c].position;
		J[c][spine_euler_y] = root_matrix * abdomen_matrix * spine_mRdy * ptrHandles[c].position;

		//h(q)
		Vec4d hq = root_matrix * abdomen_spine_matrix * ptrHandles[c].position;
		//c = h(q) - p
		Vec4d p = hm->getMarker(t, ModelerApplication::Instance()->GetFps(), c);
		if(p[0] == 0 && p[1] == 0 && p[2] == 0) {
			//cout << "***** OCCLUDED MARKER at: " << c << endl;
			ptrHandles[c].setC(Vec4d(vl_0));
		} else {
			ptrHandles[c].setC(hq - p);	
		}
	}


	/************************************************************************/
	/* Head                                                                 */
	/************************************************************************/
	
	//h_head Link Transformations
	Mat4d head_trans = translate(0.0000,1.0000*spine_len,0.0000);
	Mat4d head_rz = rotateZ(qDOF[neck_euler_z]);
	Mat4d head_rx = rotateX(qDOF[neck_euler_x]);

	Mat4d head_matrix = head_trans * head_rz * head_rx;
	Mat4d head_mRdz = head_trans * drotateZ(qDOF[neck_euler_z]) * head_rx;
	Mat4d head_mRdx = head_trans * head_rz * drotateX(qDOF[neck_euler_x]);

	//Concatenating matrices:
	Mat4d abdomen_spine_head_matrix = abdomen_spine_matrix * head_matrix;

	for(int c = LFHD; c <= RBHD; c++) {
		//Fill Jacobian d h(q)/ [dx, dy ,dz, ...]
		J[c][pelvis_trans_x] = root_mTdx * abdomen_spine_head_matrix * ptrHandles[c].position;
		J[c][pelvis_trans_y] = root_mTdy * abdomen_spine_head_matrix * ptrHandles[c].position;
		J[c][pelvis_trans_z] = root_mTdz * abdomen_spine_head_matrix * ptrHandles[c].position;
		J[c][pelvis_euler_x] = root_mRdx * abdomen_spine_head_matrix * ptrHandles[c].position;
		J[c][pelvis_euler_y] = root_mRdy * abdomen_spine_head_matrix * ptrHandles[c].position;
		J[c][pelvis_euler_z] = root_mRdz * abdomen_spine_head_matrix * ptrHandles[c].position;
		J[c][abdomen_euler_x] = root_absRdx_spine_matrix * head_matrix * ptrHandles[c].position;
		J[c][abdomen_euler_z] = root_absRdz_spine_matrix * head_matrix * ptrHandles[c].position;
		J[c][spine_euler_y] = root_matrix * abdomen_matrix * spine_mRdy * head_matrix * ptrHandles[c].position;
		J[c][neck_euler_z] = root_matrix * abdomen_spine_matrix * head_mRdz * ptrHandles[c].position;
		J[c][neck_euler_x] = root_matrix * abdomen_spine_matrix * head_mRdx * ptrHandles[c].position;

		//h(q)
		Vec4d hq = root_matrix * abdomen_spine_matrix * head_matrix * ptrHandles[c].position;
		//c = h(q) - p
		Vec4d p = hm->getMarker(t, ModelerApplication::Instance()->GetFps(), c);
		if(p[0] == 0 && p[1] == 0 && p[2] == 0) {
			//cout << "***** OCCLUDED MARKER at: " << c << endl;
			ptrHandles[c].setC(Vec4d(vl_0));
		} else {
			ptrHandles[c].setC(hq - p);	
		}
	}


	/************************************************************************/
	/* Scapula Left                                                         */
	/************************************************************************/

	//h_scapula_l Link Transformations
	Mat4d scapulaL_rot = rotateX(-0.9423);
	Mat4d scapulaL_rx = rotateX(qDOF[l_scapula_euler_x]);

	Mat4d scapulaL_matrix = scapulaL_rot * scapulaL_rx;
	Mat4d scapulaL_mRdx = scapulaL_rot * drotateX(qDOF[l_scapula_euler_x]);

	//Concatenating matrices:
	Mat4d abdomen_spine_scapulaL_matrix = abdomen_spine_matrix * scapulaL_matrix;

	for(int c = LSHO; c <= LSHO; c++) {
		//Fill Jacobian d h(q)/ [dx, dy ,dz, ...]
		J[c][pelvis_trans_x] = root_mTdx * abdomen_spine_scapulaL_matrix * ptrHandles[c].position;
		J[c][pelvis_trans_y] = root_mTdy * abdomen_spine_scapulaL_matrix * ptrHandles[c].position;
		J[c][pelvis_trans_z] = root_mTdz * abdomen_spine_scapulaL_matrix * ptrHandles[c].position;
		J[c][pelvis_euler_x] = root_mRdx * abdomen_spine_scapulaL_matrix * ptrHandles[c].position;
		J[c][pelvis_euler_y] = root_mRdy * abdomen_spine_scapulaL_matrix * ptrHandles[c].position;
		J[c][pelvis_euler_z] = root_mRdz * abdomen_spine_scapulaL_matrix * ptrHandles[c].position;
		J[c][abdomen_euler_x] = root_absRdx_spine_matrix * scapulaL_matrix * ptrHandles[c].position;
		J[c][abdomen_euler_z] = root_absRdz_spine_matrix * scapulaL_matrix * ptrHandles[c].position;
		J[c][spine_euler_y] = root_matrix * abdomen_matrix * spine_mRdy * scapulaL_matrix * ptrHandles[c].position;
		J[c][l_scapula_euler_x] = root_matrix * abdomen_spine_matrix * scapulaL_mRdx * ptrHandles[c].position;

		//h(q)
		Vec4d hq = root_matrix * abdomen_spine_scapulaL_matrix * ptrHandles[c].position;
		//c = h(q) - p
		Vec4d p = hm->getMarker(t, ModelerApplication::Instance()->GetFps(), c);
		if(p[0] == 0 && p[1] == 0 && p[2] == 0) {
			//cout << "***** OCCLUDED MARKER at: " << c << endl;
			ptrHandles[c].setC(Vec4d(vl_0));
		} else {
			ptrHandles[c].setC(hq - p);	
		}		
	}


	///************************************************************************/
	///* Scapula Right                                                        */
	///************************************************************************/

	//h_scapula_r Link Transformations
	Mat4d scapulaR_rot = rotateX(0.9423);
	Mat4d scapulaR_rx = rotateX(qDOF[r_scapula_euler_x]);

	Mat4d scapulaR_matrix = scapulaR_rot * scapulaR_rx;
	Mat4d scapulaR_mRdx = scapulaR_rot * drotateX(qDOF[r_scapula_euler_x]);

	//Concatenating matrices:
	Mat4d abdomen_spine_scapulaR_matrix = abdomen_spine_matrix * scapulaR_matrix;

	for(int c = RBAC; c <= RSHO; c+=8) {
		//Fill Jacobian d h(q)/ [dx, dy ,dz, ...]
		J[c][pelvis_trans_x] = root_mTdx * abdomen_spine_scapulaR_matrix * ptrHandles[c].position;
		J[c][pelvis_trans_y] = root_mTdy * abdomen_spine_scapulaR_matrix * ptrHandles[c].position;
		J[c][pelvis_trans_z] = root_mTdz * abdomen_spine_scapulaR_matrix * ptrHandles[c].position;
		J[c][pelvis_euler_x] = root_mRdx * abdomen_spine_scapulaR_matrix * ptrHandles[c].position;
		J[c][pelvis_euler_y] = root_mRdy * abdomen_spine_scapulaR_matrix * ptrHandles[c].position;
		J[c][pelvis_euler_z] = root_mRdz * abdomen_spine_scapulaR_matrix * ptrHandles[c].position;
		J[c][abdomen_euler_x] = root_absRdx_spine_matrix * scapulaR_matrix * ptrHandles[c].position;
		J[c][abdomen_euler_z] = root_absRdz_spine_matrix * scapulaR_matrix * ptrHandles[c].position;
		J[c][spine_euler_y] = root_matrix * abdomen_matrix * spine_mRdy * scapulaR_matrix * ptrHandles[c].position;
		J[c][r_scapula_euler_x] = root_matrix * abdomen_spine_matrix * scapulaR_mRdx * ptrHandles[c].position;

		//h(q)
		Vec4d hq = root_matrix * abdomen_spine_scapulaR_matrix * ptrHandles[c].position;
		//c = h(q) - p
		Vec4d p = hm->getMarker(t, ModelerApplication::Instance()->GetFps(), c);
		if(p[0] == 0 && p[1] == 0 && p[2] == 0) {
			//cout << "***** OCCLUDED MARKER at: " << c << endl;
			ptrHandles[c].setC(Vec4d(vl_0));
		} else {
			ptrHandles[c].setC(hq - p);	
		}	
	}


	/************************************************************************/
	/* Bicep left                                                           */
	/************************************************************************/

	Mat4d bicepL_trans = translate(0.0, 1.0*scapula_len, 0.0);
	Mat4d bicepL_rx = rotateX(qDOF[l_bicep_euler_x]);
	Mat4d bicepL_ry = rotateY(qDOF[l_bicep_euler_y]);
	Mat4d bicepL_rz = rotateZ(qDOF[l_bicep_euler_z]);

	Mat4d bicepL_matrix = bicepL_trans * bicepL_rx * bicepL_ry * bicepL_rz;
	Mat4d bicepL_mRdx = bicepL_trans * drotateX(qDOF[l_bicep_euler_x]) * bicepL_ry * bicepL_rz;
	Mat4d bicepL_mRdy = bicepL_trans * bicepL_rx * drotateY(qDOF[l_bicep_euler_y]) * bicepL_rz;
	Mat4d bicepL_mRdz = bicepL_trans * bicepL_rx * bicepL_ry * drotateZ(qDOF[l_bicep_euler_z]);

	//Concatenating matrices:
	Mat4d abdomen_spine_scapulaL_bicepL_matrix = abdomen_spine_scapulaL_matrix * bicepL_matrix;

	for(int c = LUPA; c <= LELB; c++) {
		//Fill Jacobian d h(q)/ [dx, dy ,dz, ...]
		J[c][pelvis_trans_x] = root_mTdx * abdomen_spine_scapulaL_bicepL_matrix * ptrHandles[c].position;
		J[c][pelvis_trans_y] = root_mTdy * abdomen_spine_scapulaL_bicepL_matrix * ptrHandles[c].position;
		J[c][pelvis_trans_z] = root_mTdz * abdomen_spine_scapulaL_bicepL_matrix * ptrHandles[c].position;
		J[c][pelvis_euler_x] = root_mRdx * abdomen_spine_scapulaL_bicepL_matrix * ptrHandles[c].position;
		J[c][pelvis_euler_y] = root_mRdy * abdomen_spine_scapulaL_bicepL_matrix * ptrHandles[c].position;
		J[c][pelvis_euler_z] = root_mRdz * abdomen_spine_scapulaL_bicepL_matrix * ptrHandles[c].position;
		J[c][abdomen_euler_x] = root_absRdx_spine_matrix * scapulaL_matrix * bicepL_matrix * ptrHandles[c].position;
		J[c][abdomen_euler_z] = root_absRdz_spine_matrix * scapulaL_matrix * bicepL_matrix * ptrHandles[c].position;
		J[c][spine_euler_y] = root_matrix * abdomen_matrix * spine_mRdy * scapulaL_matrix * bicepL_matrix * ptrHandles[c].position;
		J[c][l_scapula_euler_x] = root_matrix * abdomen_spine_matrix * scapulaL_mRdx * bicepL_matrix * ptrHandles[c].position;
		J[c][l_bicep_euler_x] = root_matrix * abdomen_spine_scapulaL_matrix * bicepL_mRdx * ptrHandles[c].position;
		J[c][l_bicep_euler_y] = root_matrix * abdomen_spine_scapulaL_matrix * bicepL_mRdy * ptrHandles[c].position;
		J[c][l_bicep_euler_z] = root_matrix * abdomen_spine_scapulaL_matrix * bicepL_mRdz * ptrHandles[c].position;

		//h(q)
		Vec4d hq = root_matrix * abdomen_spine_scapulaL_bicepL_matrix * ptrHandles[c].position;
		//c = h(q) - p
		Vec4d p = hm->getMarker(t, ModelerApplication::Instance()->GetFps(), c);
		if(p[0] == 0 && p[1] == 0 && p[2] == 0) {
			//cout << "***** OCCLUDED MARKER at: " << c << endl;
			ptrHandles[c].setC(Vec4d(vl_0));
		} else {
			ptrHandles[c].setC(hq - p);	
		}	
	}


	///************************************************************************/
	///* Bicep Right                                                           */
	///************************************************************************/

	Mat4d bicepR_trans = translate(0.0, 1.0*scapula_len, 0.0);
	Mat4d bicepR_rx = rotateX(qDOF[r_bicep_euler_x]);
	Mat4d bicepR_ry = rotateY(qDOF[r_bicep_euler_y]);
	Mat4d bicepR_rz = rotateZ(qDOF[r_bicep_euler_z]);

	Mat4d bicepR_matrix = bicepR_trans * bicepR_rx * bicepR_ry * bicepR_rz;
	Mat4d bicepR_mRdx = bicepR_trans * drotateX(qDOF[r_bicep_euler_x]) * bicepR_ry * bicepR_rz;
	Mat4d bicepR_mRdy = bicepR_trans * bicepR_rx * drotateY(qDOF[r_bicep_euler_y]) * bicepR_rz;
	Mat4d bicepR_mRdz = bicepR_trans * bicepR_rx * bicepR_ry * drotateZ(qDOF[r_bicep_euler_z]);

	//Concatenating matrices:
	Mat4d abdomen_spine_scapulaR_bicepR_matrix = abdomen_spine_scapulaR_matrix * bicepR_matrix;

	for(int c = RUPA; c <= RELB; c++) {
		//Fill Jacobian d h(q)/ [dx, dy ,dz, ...]
		J[c][pelvis_trans_x] = root_mTdx * abdomen_spine_scapulaR_bicepR_matrix * ptrHandles[c].position;
		J[c][pelvis_trans_y] = root_mTdy * abdomen_spine_scapulaR_bicepR_matrix * ptrHandles[c].position;
		J[c][pelvis_trans_z] = root_mTdz * abdomen_spine_scapulaR_bicepR_matrix * ptrHandles[c].position;
		J[c][pelvis_euler_x] = root_mRdx * abdomen_spine_scapulaR_bicepR_matrix * ptrHandles[c].position;
		J[c][pelvis_euler_y] = root_mRdy * abdomen_spine_scapulaR_bicepR_matrix * ptrHandles[c].position;
		J[c][pelvis_euler_z] = root_mRdz * abdomen_spine_scapulaR_bicepR_matrix * ptrHandles[c].position;
		J[c][abdomen_euler_x] = root_absRdx_spine_matrix * scapulaR_matrix * bicepR_matrix * ptrHandles[c].position;
		J[c][abdomen_euler_z] = root_absRdz_spine_matrix * scapulaR_matrix * bicepR_matrix * ptrHandles[c].position;
		J[c][spine_euler_y] = root_matrix * abdomen_matrix * spine_mRdy * scapulaR_matrix * bicepR_matrix * ptrHandles[c].position;
		J[c][r_scapula_euler_x] = root_matrix * abdomen_spine_matrix * scapulaR_mRdx * bicepR_matrix * ptrHandles[c].position;
		J[c][r_bicep_euler_x] = root_matrix * abdomen_spine_scapulaR_matrix * bicepR_mRdx * ptrHandles[c].position;
		J[c][r_bicep_euler_y] = root_matrix * abdomen_spine_scapulaR_matrix * bicepR_mRdy * ptrHandles[c].position;
		J[c][r_bicep_euler_z] = root_matrix * abdomen_spine_scapulaR_matrix * bicepR_mRdz * ptrHandles[c].position;

		//h(q)
		Vec4d hq = root_matrix * abdomen_spine_scapulaR_bicepR_matrix * ptrHandles[c].position;
		//c = h(q) - p
		Vec4d p = hm->getMarker(t, ModelerApplication::Instance()->GetFps(), c);
		if(p[0] == 0 && p[1] == 0 && p[2] == 0) {
			//cout << "***** OCCLUDED MARKER at: " << c << endl;
			ptrHandles[c].setC(Vec4d(vl_0));
		} else {
			ptrHandles[c].setC(hq - p);	
		}
	}


	/************************************************************************/
	/* Forearm Left                                                         */
	/************************************************************************/

	// h_forearm_left Link Transformations

	Mat4d forearmL_trans = translate(0.0, -1.0*bicep_len, 0.0);
	Mat4d forearmL_rz = rotateZ(qDOF[l_elbow_euler_z]);

	Mat4d forearmL_matrix = forearmL_trans * forearmL_rz;
	Mat4d forearmL_mRdz = forearmL_trans * drotateZ(qDOF[l_elbow_euler_z]);

	//Concatenating matrices:
	Mat4d abdomen_spine_scapulaL_bicepL_forearmL_matrix = abdomen_spine_scapulaL_bicepL_matrix * forearmL_matrix;

	for(int c = LFRM; c <= LWRB; c++) {
		//Fill Jacobian d h(q)/ [dx, dy ,dz, ...]
		J[c][pelvis_trans_x] = root_mTdx * abdomen_spine_scapulaL_bicepL_forearmL_matrix * ptrHandles[c].position;
		J[c][pelvis_trans_y] = root_mTdy * abdomen_spine_scapulaL_bicepL_forearmL_matrix * ptrHandles[c].position;
		J[c][pelvis_trans_z] = root_mTdz * abdomen_spine_scapulaL_bicepL_forearmL_matrix * ptrHandles[c].position;
		J[c][pelvis_euler_x] = root_mRdx * abdomen_spine_scapulaL_bicepL_forearmL_matrix * ptrHandles[c].position;
		J[c][pelvis_euler_y] = root_mRdy * abdomen_spine_scapulaL_bicepL_forearmL_matrix * ptrHandles[c].position;
		J[c][pelvis_euler_z] = root_mRdz * abdomen_spine_scapulaL_bicepL_forearmL_matrix * ptrHandles[c].position;
		J[c][abdomen_euler_x] = root_absRdx_spine_matrix * scapulaL_matrix * bicepL_matrix * forearmL_matrix * ptrHandles[c].position;
		J[c][abdomen_euler_z] = root_absRdz_spine_matrix * scapulaL_matrix * bicepL_matrix * forearmL_matrix * ptrHandles[c].position;
		J[c][spine_euler_y] = root_matrix * abdomen_matrix * spine_mRdy * scapulaL_matrix * bicepL_matrix * forearmL_matrix * ptrHandles[c].position;
		J[c][l_scapula_euler_x] = root_matrix * abdomen_spine_matrix * scapulaL_mRdx * bicepL_matrix * forearmL_matrix * ptrHandles[c].position;
		J[c][l_bicep_euler_x] = root_matrix * abdomen_spine_scapulaL_matrix * bicepL_mRdx * forearmL_matrix * ptrHandles[c].position;
		J[c][l_bicep_euler_y] = root_matrix * abdomen_spine_scapulaL_matrix * bicepL_mRdy * forearmL_matrix * ptrHandles[c].position;
		J[c][l_bicep_euler_z] = root_matrix * abdomen_spine_scapulaL_matrix * bicepL_mRdz * forearmL_matrix * ptrHandles[c].position;
		J[c][l_elbow_euler_z] = root_matrix * abdomen_spine_scapulaL_bicepL_matrix * forearmL_mRdz * ptrHandles[c].position;

		//h(q)
		Vec4d hq = root_matrix * abdomen_spine_scapulaL_bicepL_forearmL_matrix * ptrHandles[c].position;
		//c = h(q) - p
		Vec4d p = hm->getMarker(t, ModelerApplication::Instance()->GetFps(), c);
		if(p[0] == 0 && p[1] == 0 && p[2] == 0) {
			//cout << "***** OCCLUDED MARKER at: " << c << endl;
			ptrHandles[c].setC(Vec4d(vl_0));
		} else {
			ptrHandles[c].setC(hq - p);	
		}
	}


	/************************************************************************/
	/* Forearm Right                                                         */
	/************************************************************************/

	// h_forearm_right Link Transformations

	Mat4d forearmR_trans = translate(0.0, -1.0*bicep_len, 0.0);
	Mat4d forearmR_rz = rotateZ(qDOF[r_elbow_euler_z]);

	Mat4d forearmR_matrix = forearmR_trans * forearmR_rz;
	Mat4d forearmR_mRdz = forearmR_trans * drotateZ(qDOF[r_elbow_euler_z]);

	//Concatenating matrices:
	Mat4d abdomen_spine_scapulaR_bicepR_forearmR_matrix = abdomen_spine_scapulaR_bicepR_matrix * forearmR_matrix;

	for(int c = RFRM; c <= RWRB; c++) {
		//Fill Jacobian d h(q)/ [dx, dy ,dz, ...]
		J[c][pelvis_trans_x] = root_mTdx * abdomen_spine_scapulaR_bicepR_forearmR_matrix * ptrHandles[c].position;
		J[c][pelvis_trans_y] = root_mTdy * abdomen_spine_scapulaR_bicepR_forearmR_matrix * ptrHandles[c].position;
		J[c][pelvis_trans_z] = root_mTdz * abdomen_spine_scapulaR_bicepR_forearmR_matrix * ptrHandles[c].position;
		J[c][pelvis_euler_x] = root_mRdx * abdomen_spine_scapulaR_bicepR_forearmR_matrix * ptrHandles[c].position;
		J[c][pelvis_euler_y] = root_mRdy * abdomen_spine_scapulaR_bicepR_forearmR_matrix * ptrHandles[c].position;
		J[c][pelvis_euler_z] = root_mRdz * abdomen_spine_scapulaR_bicepR_forearmR_matrix * ptrHandles[c].position;
		J[c][abdomen_euler_x] = root_absRdx_spine_matrix * scapulaR_matrix * bicepR_matrix * forearmR_matrix * ptrHandles[c].position;
		J[c][abdomen_euler_z] = root_absRdz_spine_matrix * scapulaR_matrix * bicepR_matrix * forearmR_matrix * ptrHandles[c].position;
		J[c][spine_euler_y] = root_matrix * abdomen_matrix * spine_mRdy * scapulaR_matrix * bicepR_matrix * forearmR_matrix * ptrHandles[c].position;
		J[c][r_scapula_euler_x] = root_matrix * abdomen_spine_matrix * scapulaR_mRdx * bicepR_matrix * forearmR_matrix * ptrHandles[c].position;
		J[c][r_bicep_euler_x] = root_matrix * abdomen_spine_scapulaR_matrix * bicepR_mRdx * forearmR_matrix * ptrHandles[c].position;
		J[c][r_bicep_euler_y] = root_matrix * abdomen_spine_scapulaR_matrix * bicepR_mRdy * forearmR_matrix * ptrHandles[c].position;
		J[c][r_bicep_euler_z] = root_matrix * abdomen_spine_scapulaR_matrix * bicepR_mRdz * forearmR_matrix * ptrHandles[c].position;
		J[c][r_elbow_euler_z] = root_matrix * abdomen_spine_scapulaR_bicepR_matrix * forearmR_mRdz * ptrHandles[c].position;

		//h(q)
		Vec4d hq = root_matrix * abdomen_spine_scapulaR_bicepR_forearmR_matrix * ptrHandles[c].position;
		//c = h(q) - p
		Vec4d p = hm->getMarker(t, ModelerApplication::Instance()->GetFps(), c);
		if(p[0] == 0 && p[1] == 0 && p[2] == 0) {
			//cout << "***** OCCLUDED MARKER at: " << c << endl;
			ptrHandles[c].setC(Vec4d(vl_0));
		} else {
			ptrHandles[c].setC(hq - p);	
		}	
	}

	///************************************************************************/
	///* Hand Left                                                            */
	///************************************************************************/

	// h_hand_left Link Transformations

	Mat4d handL_trans = translate(0.0, -1.0*forearm_len, 0.0);
	Mat4d handL_rx = rotateX(qDOF[l_wrist_euler_x]);

	Mat4d handL_matrix = handL_trans * handL_rx;
	Mat4d handL_mRdx = handL_trans * drotateX(qDOF[l_wrist_euler_x]);

	for(int c = LFIN; c <= LFIN; c++) {
		//Fill Jacobian d h(q)/ [dx, dy ,dz, ...]
		J[c][pelvis_trans_x] = root_mTdx * abdomen_spine_scapulaL_bicepL_forearmL_matrix * handL_matrix * ptrHandles[c].position;
		J[c][pelvis_trans_y] = root_mTdy * abdomen_spine_scapulaL_bicepL_forearmL_matrix * handL_matrix * ptrHandles[c].position;
		J[c][pelvis_trans_z] = root_mTdz * abdomen_spine_scapulaL_bicepL_forearmL_matrix * handL_matrix * ptrHandles[c].position;
		J[c][pelvis_euler_x] = root_mRdx * abdomen_spine_scapulaL_bicepL_forearmL_matrix * handL_matrix * ptrHandles[c].position;
		J[c][pelvis_euler_y] = root_mRdy * abdomen_spine_scapulaL_bicepL_forearmL_matrix * handL_matrix * ptrHandles[c].position;
		J[c][pelvis_euler_z] = root_mRdz * abdomen_spine_scapulaL_bicepL_forearmL_matrix * handL_matrix * ptrHandles[c].position;
		J[c][abdomen_euler_x] = root_absRdx_spine_matrix * scapulaL_matrix * bicepL_matrix * forearmL_matrix * handL_matrix * ptrHandles[c].position;
		J[c][abdomen_euler_z] = root_absRdz_spine_matrix * scapulaL_matrix * bicepL_matrix * forearmL_matrix * handL_matrix * ptrHandles[c].position;
		J[c][spine_euler_y] = root_matrix * abdomen_matrix * spine_mRdy * scapulaL_matrix * bicepL_matrix * forearmL_matrix * handL_matrix * ptrHandles[c].position;
		J[c][l_scapula_euler_x] = root_matrix * abdomen_spine_matrix * scapulaL_mRdx * bicepL_matrix * forearmL_matrix * handL_matrix * ptrHandles[c].position;
		J[c][l_bicep_euler_x] = root_matrix * abdomen_spine_scapulaL_matrix * bicepL_mRdx * forearmL_matrix * handL_matrix * ptrHandles[c].position;
		J[c][l_bicep_euler_y] = root_matrix * abdomen_spine_scapulaL_matrix * bicepL_mRdy * forearmL_matrix * handL_matrix * ptrHandles[c].position;
		J[c][l_bicep_euler_z] = root_matrix * abdomen_spine_scapulaL_matrix * bicepL_mRdz * forearmL_matrix * handL_matrix * ptrHandles[c].position;
		J[c][l_elbow_euler_z] = root_matrix * abdomen_spine_scapulaL_bicepL_matrix * forearmL_mRdz * handL_matrix * ptrHandles[c].position;
		J[c][l_wrist_euler_x] = root_matrix * abdomen_spine_scapulaL_bicepL_forearmL_matrix * handL_mRdx * ptrHandles[c].position;

		//h(q)
		Vec4d hq = root_matrix * abdomen_spine_scapulaL_bicepL_forearmL_matrix * handL_matrix * ptrHandles[c].position;
		//c = h(q) - p
		Vec4d p = hm->getMarker(t, ModelerApplication::Instance()->GetFps(), c);
		if(p[0] == 0 && p[1] == 0 && p[2] == 0) {
			//cout << "***** OCCLUDED MARKER at: " << c << endl;
			ptrHandles[c].setC(Vec4d(vl_0));
		} else {
			ptrHandles[c].setC(hq - p);	
		}
	}


	/************************************************************************/
	/* Hand Right                                                            */
	/************************************************************************/

	// h_hand_right Link Transformations

	Mat4d handR_trans = translate(0.0, -1.0*forearm_len, 0.0);
	Mat4d handR_rx = rotateX(qDOF[r_wrist_euler_x]);

	Mat4d handR_matrix = handR_trans * handR_rx;
	Mat4d handR_mRdx = handR_trans * drotateX(qDOF[r_wrist_euler_x]);

	for(int c = RFIN; c <= RFIN; c++) {
		//Fill Jacobian d h(q)/ [dx, dy ,dz, ...]
		J[c][pelvis_trans_x] = root_mTdx * abdomen_spine_scapulaR_bicepR_forearmR_matrix * handR_matrix * ptrHandles[c].position;
		J[c][pelvis_trans_y] = root_mTdy * abdomen_spine_scapulaR_bicepR_forearmR_matrix * handR_matrix * ptrHandles[c].position;
		J[c][pelvis_trans_z] = root_mTdz * abdomen_spine_scapulaR_bicepR_forearmR_matrix * handR_matrix * ptrHandles[c].position;
		J[c][pelvis_euler_x] = root_mRdx * abdomen_spine_scapulaR_bicepR_forearmR_matrix * handR_matrix * ptrHandles[c].position;
		J[c][pelvis_euler_y] = root_mRdy * abdomen_spine_scapulaR_bicepR_forearmR_matrix * handR_matrix * ptrHandles[c].position;
		J[c][pelvis_euler_z] = root_mRdz * abdomen_spine_scapulaR_bicepR_forearmR_matrix * handR_matrix * ptrHandles[c].position;
		J[c][abdomen_euler_x] = root_absRdx_spine_matrix * scapulaR_matrix * bicepR_matrix * forearmR_matrix * handR_matrix * ptrHandles[c].position;
		J[c][abdomen_euler_z] = root_absRdz_spine_matrix * scapulaR_matrix * bicepR_matrix * forearmR_matrix * handR_matrix * ptrHandles[c].position;
		J[c][spine_euler_y] = root_matrix * abdomen_matrix * spine_mRdy * scapulaR_matrix * bicepR_matrix * forearmR_matrix * handR_matrix * ptrHandles[c].position;
		J[c][r_scapula_euler_x] = root_matrix * abdomen_spine_matrix * scapulaR_mRdx * bicepR_matrix * forearmR_matrix * handR_matrix * ptrHandles[c].position;
		J[c][r_bicep_euler_x] = root_matrix * abdomen_spine_scapulaR_matrix * bicepR_mRdx * forearmR_matrix * handR_matrix * ptrHandles[c].position;
		J[c][r_bicep_euler_y] = root_matrix * abdomen_spine_scapulaR_matrix * bicepR_mRdy * forearmR_matrix * handR_matrix * ptrHandles[c].position;
		J[c][r_bicep_euler_z] = root_matrix * abdomen_spine_scapulaR_matrix * bicepR_mRdz * forearmR_matrix * handR_matrix * ptrHandles[c].position;
		J[c][r_elbow_euler_z] = root_matrix * abdomen_spine_scapulaR_bicepR_matrix * forearmR_mRdz * handR_matrix * ptrHandles[c].position;
		J[c][r_wrist_euler_x] = root_matrix * abdomen_spine_scapulaR_bicepR_forearmR_matrix * handR_mRdx * ptrHandles[c].position;

		//h(q)
		Vec4d hq = root_matrix * abdomen_spine_scapulaR_bicepR_forearmR_matrix * handR_matrix * ptrHandles[c].position;
		//c = h(q) - p
		Vec4d p = hm->getMarker(t, ModelerApplication::Instance()->GetFps(), c);
		if(p[0] == 0 && p[1] == 0 && p[2] == 0) {
			//cout << "***** OCCLUDED MARKER at: " << c << endl;
			ptrHandles[c].setC(Vec4d(vl_0));
		} else {
			ptrHandles[c].setC(hq - p);	
		}	
	}
}

void RobotArm::optimize()
{	
	Vecd F(dofs, vl_0);

	//Calculate Gradient dF/dq
	for(int q = 0; q < dofs; q++) {
		for(int c = LFHD; c <= RMT5; c++ ) {
			if(c==LTOE || c == LHEE || c==LBHD || c==RBHD || (c== RFHD) || (c== RFRM) ) continue; // 0 Weight
			F[q] = F[q] + ( dot(J[c][q], ptrHandles[c].getC()) );
		}
	}

	//Set new q's:
	for(int q = 0; q < dofs; q++) {
		qDOF[q] -= (alpha_param * F[q]);
	}
}

void RobotArm::checkError()
{
	boolean isOptimal = false;
	float error = 0;

	int handleInit = LFHD;
	int handleEnd = RMT5;

	optimize();

	while (!isOptimal) {
		error = 0;
		for(int c = handleInit; c <= handleEnd; c++) {
			if(c==LTOE || c == LHEE || c==LBHD || c==RBHD || (c== RFHD) || (c== RFRM) ) continue; // 0 Weight
			error = error + len(ptrHandles[c].getC());
		}
		//cout << "Error: " << error << endl;
		if ((error) < error_param) {
			isOptimal = true;
		} else {
			optimize();
			computeJacobian();		
		}
	}	
}


void ground(float h) 
{
	glDisable(GL_LIGHTING);
	glColor3f(0.65,0.45,0.2);
	glPushMatrix();
	glScalef(30,0,30);
	y_box(h);
	glPopMatrix();
	glEnable(GL_LIGHTING);
}

void ellipsoid(float a, float b, float c) {
	setDiffuseColor( 0.85, 0.75, 0.25 );
	setAmbientColor( 0.95, 0.75, 0.25 );
	glPushMatrix();
	glScalef(a/2,b/2,c/2);
	drawSphere(1.0);
	glPopMatrix();
}

void y_box(float h) {

	glBegin( GL_QUADS );

	glNormal3d( 1.0 ,0.0, 0.0);			// +x side
	glVertex3d( 0.25,0.0, 0.25);
	glVertex3d( 0.25,0.0,-0.25);
	glVertex3d( 0.25,  h,-0.25);
	glVertex3d( 0.25,  h, 0.25);

	glNormal3d( 0.0 ,0.0, -1.0);		// -z side
	glVertex3d( 0.25,0.0,-0.25);
	glVertex3d(-0.25,0.0,-0.25);
	glVertex3d(-0.25,  h,-0.25);
	glVertex3d( 0.25,  h,-0.25);

	glNormal3d(-1.0, 0.0, 0.0);			// -x side
	glVertex3d(-0.25,0.0,-0.25);
	glVertex3d(-0.25,0.0, 0.25);
	glVertex3d(-0.25,  h, 0.25);
	glVertex3d(-0.25,  h,-0.25);

	glNormal3d( 0.0, 0.0, 1.0);			// +z side
	glVertex3d(-0.25,0.0, 0.25);
	glVertex3d( 0.25,0.0, 0.25);
	glVertex3d( 0.25,  h, 0.25);
	glVertex3d(-0.25,  h, 0.25);

	glNormal3d( 0.0, 1.0, 0.0);			// top (+y)
	glVertex3d( 0.25,  h, 0.25);
	glVertex3d( 0.25,  h,-0.25);
	glVertex3d(-0.25,  h,-0.25);
	glVertex3d(-0.25,  h, 0.25);

	glNormal3d( 0.0,-1.0, 0.0);			// bottom (-y)
	glVertex3d( 0.25,0.0, 0.25);
	glVertex3d(-0.25,0.0, 0.25);
	glVertex3d(-0.25,0.0,-0.25);
	glVertex3d( 0.25,0.0,-0.25);

	glEnd();
}


int main(int argc, char *argv[])
{
	std::cout << "********** IK solver **********" << std::endl;
	if(argc != 3) {
		std::cout << "You did NOT enter ALPHA and ERROR parameters. (Default values will be used)." << std::endl;
		alpha_param = 0.049;
		error_param = 0.74;
	} else {
		alpha_param = std::atof(argv[1]);
		error_param = std::atof(argv[2]);
	}
	std::cout << "Alpha value = " << alpha_param << std::endl 
		<< "Error Threshold = " << error_param << std::endl
		<< "************************" << std::endl;
	
	ModelerControl controls[NUMCONTROLS ];

	controls[BASE_ROTATION] = ModelerControl("base rotation (theta)", -180.0, 180.0, 0.1, 0.0 );
	controls[LOWER_TILT] = ModelerControl("lower arm tilt (phi)", 15.0, 95.0, 0.1, 55.0 );
	controls[UPPER_TILT] = ModelerControl("upper arm tilt (psi)", 0.0, 135.0, 0.1, 30.0 );
	controls[CLAW_ROTATION] = ModelerControl("claw rotation (cr)", -30.0, 180.0, 0.1, 0.0 );
	controls[BASE_LENGTH] = ModelerControl("base height (h1)", 0.5, 10.0, 0.1, 0.8 );
	controls[LOWER_LENGTH] = ModelerControl("lower arm length (h2)", 1, 10.0, 0.1, 3.0 );
	controls[UPPER_LENGTH] = ModelerControl("upper arm length (h3)", 1, 10.0, 0.1, 2.5 );


	HandleManager* handleManager = new HandleManager();

	ModelerApplication::Instance()->SetHandleManager(handleManager);

	ModelerApplication::Instance()->Init(&createRobotArm, controls, NUMCONTROLS);

	return ModelerApplication::Instance()->Run();
}
